
#include "libmri.h"

#include <stdarg.h>
#include <stdlib.h>
#include <math.h>

#include <algorithm>
#include <iostream>

using std::max;
using std::min;
using std::cerr;
using std::endl;
using std::cout;

void calib2d_atamul (cplxf *z, cplxf *calib, int *csize, cplxf *x, int *xsize);


void
calib2d_lanczos (cplxf *V, float *D, cplxf *alpha, float *beta, cplxf *Q,
                 cplxf *calib, int *csize, int *ksize, int k, cplxf *b)
{
	double beta_tol = 1e-6;

	int nch = csize[2],
	    ks1 = ksize[0],
	    ks2 = ksize[1];

	int n = ks1 * ks2 * nch;

	for (int i = 0; i < n*k; ++i){ Q[i].real = Q[i].imag = 0.0f; }
	for (int i = 0; i < k; ++i){ alpha[i] = 0.0f; }
	for (int i = 0; i < k-1; ++i){ beta[i] = 0.0f; }

	double bnrm = 0; for (int i = 0; i < n; ++i){ bnrm += abs2 (b[i]); }
	bnrm = sqrt(bnrm);
	for (int i = 0; i < n; ++i){
		Q[i] = b[i] * (1.0 / bnrm);
	}

	cplxf *z = new cplxf[n];

	for (int j = 0; j < k; ++j)
	{
		calib2d_atamul (z, calib, csize, &Q[j*n], ksize);

		alpha[j] = cdotc (n, (complex*) &Q[j*n], 1, (complex*) z, 1);

		if (j == 26){
			debug_shm("z26_pre", z, 2, n, 1); 
			debug_shm("Q26_pre", Q, 2, n, j);
		}

		for (int t = 0; t < 2; ++t) // reorthogonalize {\emph twice}!
		{
			cplxf *w = new cplxf [j+1];

			complex _1[1] = {{1.0f,0.0f}},
			        _0[1] = {{0.0f,0.0f}},
			       _n1[1] = {{-1.0f, 0.0f}};

			cgemv ('C', n, j+1,  _1, (complex*) Q, n, (complex*) z, 1,
			        _0, (complex*) w, 1);

			cgemv ('N', n, j+1, _n1, (complex*) Q, n, (complex*) w, 1,
			        _1,	(complex*) z, 1);

			if (j == 26){
				char buf[1024];
				sprintf (buf, "w26_%d", t);
				debug_shm (buf, w, 2, j+1,1);
			}

			delete [] w;
		}

		if (j == 26){
			debug_shm("z26_post", z, 2, n, 1); 
			debug_shm("Q26_post", Q, 2, n, j);
		}

		double znrm = 0;
		for (int i = 0; i < n; ++i){ znrm += abs2 (z[i]); }
		znrm = sqrt (znrm);

		if (j == 26){
			debug_shm("znrm26", &znrm, 2, 1, 1); 
		}

		beta[j] = znrm;

		if (beta[j] < beta_tol) break;

		if (j < k-1){
			for (int i = 0; i < n; ++i)
				Q[i + n*(j+1)] = z[i] * (1.0 / znrm);
		}
	}

	for (int i = 0; i < n; ++i){ D[i] = alpha[i].real; }
	for (int i = 0; i < k*n; ++i){ V[i] = Q[i]; }
	int info = 0;
	csteqr ('V', k, (float*) D, beta, (complex*) V, n, &info);

	if (info != 0){
		cerr << "csteqr info=>" << info << endl;
	}
}

#ifdef _MAIN_

int main (int argc, char **argv)
{
	shm_array_t *shm_calib = 0, *shm_b = 0;
	int *ksize = 0, nksize = 0;
	int k = 0;

	struct arg *args [] = {
		new arg_t <shm_array_t*> ("calib", shm_calib),
		new list_arg_t <int> ("ksize", ksize, nksize, "x"),
		new arg_t <int> ("k", k),
		0
	};

	get_args (argc, argv, args);

	if (shm_calib == 0){
		if (get_shm_array ("calib", &shm_calib)){
			perror("calib");
			return -1;
		}
	}
	cplxf *calib = (cplxf*) shm_calib->data.c;

	int cs1 = shm_calib->dims[0],
	    cs2 = shm_calib->dims[1],
	    nch = shm_calib->dims[2];
	int csize[3] = {cs1, cs2, nch};

	if (shm_b == 0){
		if (get_shm_array ("b", &shm_b)){
			perror("b");
			return -1;
		}
	}
	cplxf *b = (cplxf*) shm_b->data.c;

	if (ksize == 0){
		ksize = new int[2];
		ksize[0] = ksize[1] = 7;
	}
	int ks1 = ksize[0], ks2 = ksize[1];

	int n = ks1 * ks2 * nch;

	if (k == 0) k = 100;


#define output_shm(name,ctype,shmtype,_ndims,sz1,sz2,sz3,sz4) \
	shm_array_t shmsz_##name, *shm_##name; \
	shmsz_##name.ndims = _ndims; \
	shmsz_##name.dims[0] = sz1; \
	shmsz_##name.dims[1] = sz2; \
	shmsz_##name.dims[2] = sz3; \
	shmsz_##name.dims[3] = sz4; \
	shmsz_##name.type = shm_array_##shmtype; \
	if (make_shm_array (#name , &shmsz_##name, &shm_##name)){ \
		perror ("make_shm_array(" #name ")"); \
		return -1; \
	} \
	ctype *name = (ctype*) shm_##name->data.c;

	output_shm (Q,     cplxf,cplxf, 2, n,   k, 1,1)
	output_shm (alpha, cplxf,cplxf, 2, k,   1, 1,1)
	output_shm (beta,  float,float, 2, k-1, 1, 1,1)
	output_shm (D,     float,float, 2, k,   1, 1,1)
	output_shm (V,     cplxf,cplxf, 2, n,   k, 1,1)

	calib2d_lanczos (V, D, alpha, beta, Q, calib, csize, ksize, k, b);

	return 0;
}

#endif
